#include "StdAfx.h"
#include "AlgoritmHandler.h"
#include "Utility.h"
#include "AntXDlg.h"
vector<NodePoint> AlgoritmHandler::GlobalNodesTable;
Color AlgoritmHandler::DefaultLineColor=Color(0,0,0);
int AlgoritmHandler::DefaultLineWidth=1;
vector<Line> AlgoritmHandler::GlobalLinePairs;
set<int> AlgoritmHandler::ForbiddenNodes;
queue<NODENUM> AlgoritmHandler::mBestRoadNodesQue;



AlgoritmHandler::AlgoritmHandler(void)
{
	mBestRoadLength=INT_MAX;
}

AlgoritmHandler::~AlgoritmHandler(void)
{
}

void AlgoritmHandler::DrawLineFromNodeToNode( Graphics* graphics, NodePoint& oriNodePoint, NodePoint& disNodePoint )
{
	Point2d lineStartPos=oriNodePoint.CalculateOutgoingLineStartPosition(disNodePoint.getNodePointCenter());
	Point2d lineEndPos=disNodePoint.CalculateOutgoingLineStartPosition(oriNodePoint.getNodePointCenter());
	Pen linePen(DefaultLineColor, DefaultLineWidth);
	graphics->DrawLine(&linePen, lineStartPos.x, lineStartPos.y, lineEndPos.x, lineEndPos.y);

}

void AlgoritmHandler::DrawLineFromNodeToNode( Graphics* graphics, Color theColor, NodePoint& oriNodePoint, NodePoint& disNodePoint )
{
	Point2d lineStartPos=oriNodePoint.CalculateOutgoingLineStartPosition(disNodePoint.getNodePointCenter());
	Point2d lineEndPos=disNodePoint.CalculateOutgoingLineStartPosition(oriNodePoint.getNodePointCenter());
	Pen linePen(theColor, DefaultLineWidth);
	graphics->DrawLine(&linePen, lineStartPos.x, lineStartPos.y, lineEndPos.x, lineEndPos.y);

}

void AlgoritmHandler::setDefaultLineColor( Color theColor )
{
	AlgoritmHandler::DefaultLineColor=theColor;
}

void AlgoritmHandler::setDefaultLineWidth( int lineWidth )
{
	AlgoritmHandler::DefaultLineWidth=lineWidth;
}

void AlgoritmHandler::setDefaultNodeColor( Color theColor )
{
	for (size_t i=0; i<AlgoritmHandler::GlobalNodesTable.size(); ++i)
	{
		GlobalNodesTable[i].setNodePointColor(theColor);
	}
}

void AlgoritmHandler::InitGlobalLinePairsWithNodesTable()
{
	for (size_t i=0; i<AlgoritmHandler::GlobalNodesTable.size(); ++i)
	{
		NodePoint theNode=GlobalNodesTable[i];
		for (size_t j=0; j<theNode.mNeighbourInfoArray.size(); ++j)
		{
			NeighbourInfo theNeighbour=theNode.mNeighbourInfoArray[j];
			Line theLine;
			theLine.linePair=make_pair(theNode.getNodeNum(), theNeighbour.neighNum);
			theLine.Dis=theNeighbour.distance;
			if (!IsInGlobalLinePairs(theLine))
			{
				GlobalLinePairs.push_back(theLine);
			}
		}
	}
}

bool AlgoritmHandler::IsInGlobalLinePairs(Line theLine)
{
	for (size_t i=0; i<GlobalLinePairs.size(); ++i)
	{
		Line curLine=GlobalLinePairs[i];
		if (curLine.linePair.first==theLine.linePair.first&&
			curLine.linePair.second==theLine.linePair.second||
			curLine.linePair.second==theLine.linePair.first&&
			curLine.linePair.first==theLine.linePair.second)
		{
			return true;
		}

	}
	return false;
}

void AlgoritmHandler::DrawLines(Graphics* graphics)
{
	 for (size_t i=0; i<GlobalLinePairs.size(); ++i)
	 {
		 Line theLine=GlobalLinePairs[i];
		 //draw the line
		 DrawLineFromNodeToNode(graphics, GlobalNodesTable[theLine.linePair.first-1],
			  GlobalNodesTable[theLine.linePair.second-1]);

		 //draw the corresponding line distance
		 FontFamily  fontFamily(L"Times New Roman");
		 Font        font(&fontFamily, 15, FontStyleRegular, UnitPixel);
		 int x1=GlobalNodesTable[theLine.linePair.first-1].getNodePointCenter().x;
		 int x2=GlobalNodesTable[theLine.linePair.second-1].getNodePointCenter().x;
		 int y1=GlobalNodesTable[theLine.linePair.first-1].getNodePointCenter().y;
		 int y2=GlobalNodesTable[theLine.linePair.second-1].getNodePointCenter().y;
		 PointF      pointF((x1+x2)/2+8, (y1+y2)/2+8);
		 SolidBrush  solidBrush(Color(255, 0, 0, 255));
		 string distance=Utility::ConvertNumberToString(theLine.Dis);
		 WCHAR wc[100]; 
		 swprintf(wc,L"%S",distance.c_str());    
		 graphics->DrawString(wc, -1, &font, pointF, &solidBrush);
	 }
	if (!mBestRoadNodesQue.empty())
	{
		while (mBestRoadNodesQue.front()!=getMainDlg()->mEndNode)
		{
			NODENUM theFisrt=mBestRoadNodesQue.front();
			mBestRoadNodesQue.pop();
			NODENUM theSecond=mBestRoadNodesQue.front();
			if (IsPairInGlobalLinePair(theFisrt, theSecond))
			{
				DrawLineFromNodeToNode(graphics, Color(0,255,0), GlobalNodesTable[theFisrt-1],
					GlobalNodesTable[theSecond-1]);
			}
		}
	}
}

void AlgoritmHandler::ClearAllDatas()
{
	AlgoritmHandler::GlobalNodesTable.clear();
	AlgoritmHandler::GlobalLinePairs.clear();
}

bool AlgoritmHandler::IsInSet( set<int>& theSet, int theElement )
{
	if (theSet.find(theElement)!=theSet.end())
	{
		return true;
	}
	else
		return false;
}

NODENUM AlgoritmHandler::ChooseNextNode( NODENUM curNodeNum, vector<NodePoint>& theTable )
{
	size_t vsize=theTable[curNodeNum-1].mNeighbourInfoArray.size();
	vector<pair<NODENUM, float>> proVector;
	float sum=0.0;
	for (size_t i=0; i<vsize; ++i)
	{
		NeighbourInfo theNeighbour=theTable[curNodeNum-1].mNeighbourInfoArray[i];
		NODENUM theNext=theNeighbour.neighNum;
		if (IsInSet(mVisitedNodes, theNext)||IsInSet(ForbiddenNodes, theNext))
			continue;
		else
		{
			float probability=CalculateNeighbourNodesProbability(curNodeNum,theNext);
			proVector.push_back(make_pair(theNext, probability));
			sum+=probability;
		}
	}
	//a failed search
	if (proVector.empty())
	{
		return INVALIDNODE;
	}
	for (size_t i=0; i<proVector.size(); i++)
	{
		pair<NODENUM, float>& thePair=proVector[i];
		thePair.second/=sum;
	}
	return RouletteChoose(proVector);
}

float AlgoritmHandler::GetNeighbourNodesInfo( NODENUM first, NODENUM second, int type/*=TYPEDIS*/ )
{
	for (size_t i=0; i<GlobalNodesTable[first-1].mNeighbourInfoArray.size(); ++i )
	{
		NeighbourInfo theNeighbour=GlobalNodesTable[first-1].mNeighbourInfoArray[i];
		if (theNeighbour.neighNum==second)
		{
			if (type==TYPEDIS)
				return theNeighbour.distance;
			if (type==TYPEPPHEROMONE)
				return theNeighbour.pheromone;
		}
	}
}

void AlgoritmHandler::SetNeighbourNodesInfo( NODENUM first, NODENUM second, float theValue, int type/*=TYPEDIS*/ )
{
	for (size_t i=0; i<GlobalNodesTable[first-1].mNeighbourInfoArray.size(); ++i )
	{
		NeighbourInfo& theNeighbour=GlobalNodesTable[first-1].mNeighbourInfoArray[i];
		if (theNeighbour.neighNum==second)
		{
			if (type==TYPEDIS)
				theNeighbour.distance=theValue;
			if (type==TYPEPPHEROMONE)
				theNeighbour.pheromone=theValue;
		}
	}
}

float AlgoritmHandler::CalculateNeighbourNodesProbability( NODENUM curNode, NODENUM next )
{
	//according to the reference
	float Pij=0.0;
	float Tij=GetNeighbourNodesInfo(curNode, next, TYPEPPHEROMONE);
	float Dij=GetNeighbourNodesInfo(curNode, next, TYPEDIS);
	Pij=pow(Tij,(float)(getMainDlg()->mAlpha))/pow(Dij,(float)(getMainDlg()->mBeta));
	return Pij;
}

NODENUM AlgoritmHandler::RouletteChoose( vector<pair<NODENUM, float>>& theCandidates )
{
	float randNumber=RangedRand(0.0, 1.0);
	float temp=0.0;
	for (size_t i=0; i<theCandidates.size(); ++i)
	{
		temp+=theCandidates[i].second;
		if (randNumber<=temp)
		{
			return theCandidates[i].first;
		}
	}
}

float AlgoritmHandler::RangedRand( float range_min, float range_max )
{
	float ran = (float)rand() / (RAND_MAX + 1) * (range_max - range_min)+ range_min;
	return ran;
}

void AlgoritmHandler::DoAlgorithm()
{
	PreSetParams();
	for (int i=0; i<getMainDlg()->mItCount; ++i)
	{
		RunTheAnts();
	}
	//mark the best road nods as red
	while (mBestRoadNodesQue.front()!=getMainDlg()->mEndNode)
	{
		GlobalNodesTable[mBestRoadNodesQue.front()-1].setNodePointColor(Color(0,255,0));
		mBestRoadNodesQue.push(mBestRoadNodesQue.front());
		mBestRoadNodesQue.pop();
	}
	GlobalNodesTable[mBestRoadNodesQue.front()-1].setNodePointColor(Color(0,255,0));
	mBestRoadNodesQue.push(mBestRoadNodesQue.front());
	mBestRoadNodesQue.pop();

}

void AlgoritmHandler::VolatilizePheromone()
{
	for (size_t i=0; i<GlobalNodesTable.size(); ++i)
	{
		NeighbourInfoArray& theNeighbourArray=GlobalNodesTable[i].mNeighbourInfoArray;
		for (size_t j=0; j<theNeighbourArray.size(); ++j)
		{
			NeighbourInfo& theNeighbour=theNeighbourArray[j];
			theNeighbour.pheromone*=(1-getMainDlg()->mROU);
		}
	}
}

void AlgoritmHandler::RunTheAnts()
{
	vector<NodePoint> tempTable=GlobalNodesTable;
	bool isVolatilized=false;
	srand((unsigned)time( NULL ));
	for (int count=0; count<getMainDlg()->mAntCount; ++count)
	{
		mVisitedNodes.clear();
		// for an ant
		float roadLength=0.0;
		queue<NODENUM> roadNodesQue;		
		NODENUM curNodeName=getMainDlg()->mStartNode;
		roadNodesQue.push(curNodeName);
		while (curNodeName!=getMainDlg()->mEndNode)
		{
			mVisitedNodes.insert(curNodeName);
			NODENUM nextNodeName=ChooseNextNode(curNodeName, tempTable);
			//a failed road
			if (nextNodeName==INVALIDNODE)
			{
				while(roadNodesQue.front()!=curNodeName)
				{
					roadNodesQue.push(roadNodesQue.front());
					roadNodesQue.pop();
				}
				roadNodesQue.pop();
				curNodeName=roadNodesQue.back();
				continue;
			}
			roadNodesQue.push(nextNodeName);
			roadLength+=GetNeighbourNodesInfo(curNodeName, nextNodeName, TYPEDIS);
			curNodeName=nextNodeName;
		}
		//for debug
		queue<int> temp=roadNodesQue;
		while (temp.front()!=getMainDlg()->mEndNode)
		{
			TRACE("%d", temp.front());
			temp.push(temp.front());
			temp.pop();
		}
		TRACE("%d", temp.front());
		TRACE("\n");
		//save the best road nodes and length
		if (roadLength<mBestRoadLength)
		{
			mBestRoadLength=roadLength;
			mBestRoadNodesQue=roadNodesQue;
		}
		//update the pheromone
		while (roadNodesQue.front()!=getMainDlg()->mEndNode)
		{
			if (!isVolatilized)
			{
				VolatilizePheromone();
				isVolatilized=true;
			}
			NODENUM firstNodeName=roadNodesQue.front();
			roadNodesQue.pop();
			NODENUM secondNodeName=roadNodesQue.front();
			float oldValue1=GetNeighbourNodesInfo(firstNodeName, secondNodeName, TYPEPPHEROMONE);
			SetNeighbourNodesInfo(firstNodeName, secondNodeName, oldValue1+(getMainDlg()->mQ/roadLength), TYPEPPHEROMONE);
			float oldValue2=GetNeighbourNodesInfo(secondNodeName, firstNodeName, TYPEPPHEROMONE);
			SetNeighbourNodesInfo(secondNodeName, firstNodeName, oldValue2+(getMainDlg()->mQ/roadLength), TYPEPPHEROMONE);
		}
	}//end for the ants
}

CAntXDlg* AlgoritmHandler::getMainDlg()
{
	return (CAntXDlg* )AfxGetMainWnd();
}

void AlgoritmHandler::PreSetParams()
{
	mBestRoadLength=INT_MAX;
	mBestRoadNodesQue=queue<NODENUM>();
	for (size_t i=0; i<GlobalNodesTable.size(); ++i)
	{
		GlobalNodesTable[i].setNodePointColor(Color(0,0,255));
		NeighbourInfoArray& infoArray=GlobalNodesTable[i].mNeighbourInfoArray;
		for (size_t j=0; j<infoArray.size(); ++j)
		{
			NeighbourInfo& info=infoArray[j];	
			info.pheromone=1.0;
		}
	}
}

bool AlgoritmHandler::IsPairInGlobalLinePair( NODENUM theFirst, NODENUM theSecond )
{
	if (!mBestRoadNodesQue.empty())
	{
		Line theLine;
		theLine.linePair=make_pair(theFirst, theSecond);
		theLine.Dis=0.0;
		return IsInGlobalLinePairs(theLine);
	}
}

 


 

 
